#include "attachobjectmanipulator.h"
#include <QString>
#include <osg/math>
#include "pickmanager.h"
//#include "terrainmanager.h"

namespace ControlCenter
{
	AttachObjectManipulator::AttachObjectManipulator()
	{
		//_eyeVec = osg::Vec3(5470,0,_cameraHeight);
		//_centerVec = osg::Vec3(0,0,0);
		//_upVec = osg::Vec3(0,0,1);
		useCollisionCamera(false);
	}

	AttachObjectManipulator::~AttachObjectManipulator()
	{}

	void AttachObjectManipulator::objectTransformInitThis(const osg::Vec3& eyepos,const osg::Vec3& pos,const osg::Vec3& offset,const osg::Vec3& rota,const osg::Vec3& up)
	{
		_homeEye = eyepos + pos + offset;
		_homeUp = up;
		_homeCenter = pos + offset;
		_baseEye = eyepos + offset;
		_direction = _homeCenter - _homeEye;
		_direction.normalize();
		_directionBase = _direction;
		_position = _homeEye;
		_baseUp = _homeUp;
	}

	void AttachObjectManipulator::setObjectTransform(const osg::Vec3& pos,const osg::Vec3& rota)
	{
		_direction = _directionBase * osg::Matrix::rotate(rota.z()*osg::PI/180.0f, osg::Vec3(0,0,1));
		_homeUp =  _baseUp * osg::Matrix::rotate(rota.z()*osg::PI/180.0f, osg::Vec3(0,0,1));
		_position = _baseEye*osg::Matrix::rotate(rota.z()*osg::PI/180.0f, osg::Vec3(0,0,1))+pos;

		/*if( fabs(_directionRotationRate + _rightMouseRotationRate) > _directionRotationEpsilon)
		{
		_direction = _direction * osg::Matrix::rotate(_directionRotationRate + _rightMouseRotationRate, osg::Vec3(0,0,1));
		_homeUp = _homeUp *osg::Matrix::rotate(_directionRotationRate + _rightMouseRotationRate, osg::Vec3(0,0,1));
		}
		osg::Vec3d _sideVec = _direction * osg::Matrix::rotate( -M_PI*0.5, _homeUp);
		_direction = _direction *osg::Matrix::rotate(_upDisctence, _sideVec);
		_homeUp = _homeUp *osg::Matrix::rotate(_upDisctence, _sideVec);
		*/
		_inverseMatrix.makeLookAt( _position, _position + _direction, _homeUp);
	}

	void AttachObjectManipulator::home(double)
	{
		_position = _homeEye;
		_direction = _homeCenter - _homeEye;
		_direction.normalize();
		_directionBase = _direction;
		_directionRotationRate = 0.0;
		_rightMouseRotationRate = 0.0f;
		_inverseMatrix.makeLookAt( _homeEye, _homeCenter, _homeUp );
		_forwardSpeed = 0.0;
		_sideSpeed = 0.0;
		_upSpeed = 0.0;
	}

	void AttachObjectManipulator::_frame()
	{
		if(_lostFocus)
		{
			_rightMouseRotationRate = 0.0f;
			_directionRotationRate = 0.0;
			_forwardSpeed = 0.0;
			_sideSpeed = 0.0;
			_upSpeed = 0.0;
			_upDisctence = 0.0f;
			/*_collisionForwardSpeed = 0.0;
			_collisionSideSpeed = 0.0;*/
			_collisionOffsetVec3 = osg::Vec3(0,0,0);
			return;
		}
		_rightMouseRotationRate = 0.0f;
		_upDisctence = 0.0f;
		/*_collisionForwardSpeed = 0.0;
		_collisionSideSpeed = 0.0;*/
		_collisionOffsetVec3 = osg::Vec3(0,0,0);
	}

	osg::Matrixd AttachObjectManipulator::getInverseMatrix() const
	{
		return _inverseMatrix;
	}
}